#ifndef __LIDAR_UNDISORTION_2D_H_
#define __LIDAR_UNDISORTION_2D_H_

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>

#include "../core/core.h"
#include <iostream>
#include <vector>
#include <boost/circular_buffer.hpp>
using namespace std;

// https://blog.csdn.net/elephant_why/article/details/107131518
// https://github.com/elewu/2d_lidar_undistortion

typedef boost::circular_buffer<sensor_msgs::Imu> ImuCircularBuffer;

class Lidar2DUndistortion{
    public:
        Lidar2DUndistortion();
        ~Lidar2DUndistortion();

        // 添加IMU数据
        void PushImuData(const sensor_msgs::ImuConstPtr& msg);
        // 处理雷达数据
        void HandleLidarScanData(const sensor_msgs::LaserScanConstPtr& msg, 
            sensor_msgs::LaserScan& output_msg);
    private:
        ros::NodeHandle __nh;
        ImuCircularBuffer __imu_cache_buffer;
        int __imu_max_mums;
        sensor_msgs::LaserScan __scan_undistortion;

};

#endif
